#include <iostream>
#include <opencv2/opencv.hpp>
#include <math.h>
#include "timer.hpp"
#include "energy.hpp"
#include "targetprediction.hpp"

using namespace cv;
using std::cout;
using std::endl;
int main(){
    Timer clock;
    VideoCapture cap("outcpp3.avi");
    VideoWriter output;
    Mat map(Size(640, 360), CV_8UC1);
    Energy energy;
    TargetPrediction tp;
    output.open("效果.avi", CV_FOURCC('M','P','4','2'), 25, Size(640, 360));
    while (true) {
        Mat src_image;
        Mat bin_image;
        if (!cap.read(src_image)) {
            cout << "读取源图像失败！\n";
            break;
        }
        resize(src_image, src_image, Size(640, 360));
        if (energy.findCurrentArmor(src_image)) {
            circle(src_image, energy.target_energy_armor.center, 2, Scalar(0, 255, 0), 2);
            energy.findCenterR(src_image);
        }

        //绘制拟合圆
        if (energy.isFindCenterR) {
            circle(src_image, energy.circle_center, energy.circle_radius, Scalar(255, 255, 255), 1);
            circle(src_image, energy.circle_center, 2, Scalar(255, 255, 255), 2);
        }
        if (energy.isFindTatgetArmor && energy.isFindCenterR) {
            tp.toPolarCoordinates(energy.target_energy_armor.center, energy.circle_center, energy.circle_radius);
            if (tp.direction != UNKNOWN ) {
                tp.predicting(energy);
                circle(src_image, energy.predicted_point, 2, Scalar(255, 255, 0), 2);
            }
        }
        cout<<tp.direction<<endl;

        imshow("BGR", src_image);
//        waitKey(0);
//        waitKey(1000 / 50);
        output<<src_image;
    }
    output.release();
    clock.stop();
    waitKey();
    return 0;
}